package com.hitqz.robot.biz.business.model;

import lombok.Data;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.concurrent.PriorityBlockingQueue;

@Data
public class AStar {
    // 0是路 1是墙 2是起点 3是终点
    private int[][] map;
    // 起点坐标
    int startI;
    int startJ;
    // 终点坐标
    int endI;
    int endJ;

    public AStar(int[][] map) {
        this.map = map;
        // 获取起点和终点坐标
        for (int i = 0; i < map.length; i++) {
            for (int j = 0; j < map[i].length; j++) {
                if(map[i][j]==2){
                    startI = i;
                    startJ = j;
                }
                if(map[i][j]==3){
                    endI = i;
                    endJ = j;
                }
            }
        }
    }

    public void solve(){
        boolean[][] active = new boolean[map.length][map[0].length];
        PriorityBlockingQueue<Node> priorityBlockingQueue = new PriorityBlockingQueue<>();
        // 从起点出发
        ArrayList<int[]> initPath = new ArrayList<>();
        initPath.add(new int[]{startI,startJ});
        priorityBlockingQueue.add(new Node(startI,startJ,initPath,caculateDistance(startI,startJ)));
        active[startJ][startJ] = true;
        // 开始循环
        while (!priorityBlockingQueue.isEmpty()){
            Node node = priorityBlockingQueue.poll();
            if(node.getI()==endI && node.getJ()==endJ){
                for (int[] p : node.getPath()) {
                    System.out.println(Arrays.toString(p));
                }
                System.out.println("最短路长度为："+node.getPath().size());
                break;
            }
            // 向四周进行扩充
            // 上
            int i1 = node.getI()-1;
            int j1 = node.getJ();
            if(isAble(i1,j1,active)){
                ArrayList<int[]> path = new ArrayList<>(node.getPath());
                path.add(new int[]{i1,j1});
                priorityBlockingQueue.add(new Node(i1,j1,path,caculateDistance(i1,j1)));
                active[i1][j1] = true;
            }
            // 下
            int i2 = node.getI()+1;
            int j2 = node.getJ();
            if(isAble(i2,j2,active)){
                ArrayList<int[]> path = new ArrayList<>(node.getPath());
                path.add(new int[]{i2,j2});
                priorityBlockingQueue.add(new Node(i2,j2,path,caculateDistance(i2,j2)));
                active[i2][j2] = true;
            }
            // 左
            int i3 = node.getI();
            int j3 = node.getJ()-1;
            if(isAble(i3,j3,active)){
                ArrayList<int[]> path = new ArrayList<>(node.getPath());
                path.add(new int[]{i3,j3});
                priorityBlockingQueue.add(new Node(i3,j3,path,caculateDistance(i3,j3)));
                active[i3][j3] = true;
            }
            // 右
            int i4 = node.getI();
            int j4 = node.getJ()+1;
            if(isAble(i4,j4,active)){
                ArrayList<int[]> path = new ArrayList<>(node.getPath());
                path.add(new int[]{i4,j4});
                priorityBlockingQueue.add(new Node(i4,j4,path,caculateDistance(i4,j4)));
                active[i4][j4] = true;
            }
        }
    }
    // 判断坐标是否可行
    private boolean isAble(int i,int j,boolean[][] active){
        if(i<0 || i>=map.length){
            return false;
        }
        if(j<0 || j>= map[0].length){
            return false;
        }
        return map[i][j] != 1 && !active[i][j];
    }
    // 计算距离终点的曼哈顿
    private int caculateDistance(int i,int j){
        return Math.abs(i-endI)+Math.abs(j-endJ);
    }



    public static void main(String[] args) {
        long start = System.currentTimeMillis();
        int[][] map = new int[][]{
                {1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 1, 1, 0, 0, 0},
                {0, 0, 0, 0, 2, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0},
                {0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 1, 0},
                {0, 0, 1, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0},
                {0, 1, 1, 1, 0, 1, 0, 0, 1, 1, 1, 0, 0, 1, 0, 0, 0, 0, 1, 0},
                {0, 0, 0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0},
                {0, 0, 0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0},
                {0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 1, 1, 1, 1, 0},
                {0, 1, 1, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0},
                {0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0},
                {0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0},
                {0, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 1, 1, 0, 1, 0},
                {0, 0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0},
                {0, 0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0},
                {0, 0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0},
                {0, 0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 1, 0, 1, 1, 1},
                {0, 0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 1},
                {0, 0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 1},
                {0, 0, 1, 0, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1},
                {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 3, 0, 0, 0, 0, 0, 0, 0},
        };
        new AStar(map).solve();
        System.out.println("用时："+(System.currentTimeMillis()-start)+"ms");
    }


}
